#include "MyCAN.h"
void CAN_Config(CAN_HandleTypeDef *hcan)
{	

	CAN_FilterTypeDef  sFilterConfig;

  /*配置CAN过滤器*/
  sFilterConfig.FilterBank = 0;                     //过滤器0
  sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
  sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
  sFilterConfig.FilterIdHigh = 0x0000;              //32位ID
  sFilterConfig.FilterIdLow = 0x0000;
  sFilterConfig.FilterMaskIdHigh = 0x0000;          //32位MASK
  sFilterConfig.FilterMaskIdLow = 0x0000;
  sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;//过滤器0关联到FIFO0
  sFilterConfig.FilterActivation = ENABLE;          //激活滤波器0
  sFilterConfig.SlaveStartFilterBank = 14;	
	
 if(HAL_CAN_ConfigFilter(hcan,&sFilterConfig) != HAL_OK)//初始化过滤器
 {
  Error_Handler();
 }
 if(HAL_CAN_Start(hcan) != HAL_OK)//打开can
 {
  Error_Handler();
 }
 if(HAL_CAN_ActivateNotification(hcan,CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)//开启接受邮邮箱0挂起中断
 {
  Error_Handler();
 }
}


void CAN_senddata(CAN_HandleTypeDef *hcan,uint32_t can_id)
{
     TXHeader.StdId=can_id ;
	 TXHeader.ExtId=0;
	 TXHeader.DLC=8;
	 TXHeader.IDE=CAN_ID_STD;
	 TXHeader.RTR=CAN_RTR_DATA;
	 TXHeader.TransmitGlobalTime = DISABLE;
     HAL_CAN_AddTxMessage(hcan,&TXHeader,TXmessage,&pTxMailbox);
  
}

uint32_t CAN1_Receive_Msg(uint8_t *buf)
{
 	uint32_t i;
	uint8_t	RxData[8];

	if(HAL_CAN_GetRxFifoFillLevel(&hcan1, CAN_RX_FIFO0) != 1)//没有接收到数据,直接退出 
	{
		return 0xF1;
	}

	if(HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &RXHeader, RxData) != HAL_OK)
	{
		return 0xF2;
	}
	
    for(i=0;i<RXHeader.DLC;i++)
    buf[i]=RxData[i];
	  return RXHeader.DLC;
}


///*CAN发送帧*/
//void MyCAN_Transmit(CanTxMsg *TxMessage)
//{
//	uint8_t TransmitMailbox = CAN_Transmit(CAN1, TxMessage);
//	
//	uint32_t Timeout = 0;
//	while (CAN_TransmitStatus(CAN1, TransmitMailbox) != CAN_TxStatus_Ok)
//	{
//		Timeout ++;
//		if (Timeout > 500)
//		{
//			break;
//		}
//	}
//}



//uint8_t MyCAN_ReceiveFlag(void)
//{
//	if (CAN_MessagePending(CAN1, CAN_FIFO0) > 0)
//	{
//		return 1;
//	}
//	return 0;
//}
///*CAN接收帧，解析帧到结构体中*/
//void MyCAN_Receive(CanRxMsg *RxMessage)
//{
//	CAN_Receive(CAN1, CAN_FIFO0, RxMessage);
//}


/*设置电机的发送电流值，并发送*/
void Set_moto_current(CAN_TxHeaderTypeDef *TxD, moto_current *M_C)
	{

	TxD->StdId = 0x200;
	TxD->IDE = CAN_ID_STD;
	TxD->RTR = CAN_RTR_DATA;
	TxD->DLC = 0x08;
	TXmessage[0] = M_C->M1_current>> 8;
	TXmessage[1] = M_C->M1_current;
	TXmessage[2] = M_C->M2_current>> 8;
	TXmessage[3] = M_C->M2_current;
	TXmessage[4] = M_C->M3_current>> 8;
	TXmessage[5] = M_C->M3_current;
	TXmessage[6] = M_C->M4_current>> 8;
	TXmessage[7] = M_C->M4_current;		
    HAL_CAN_AddTxMessage(&hcan1,TxD,TXmessage,&pTxMailbox);
}	

void Get_moto_offset(moto_measure_t *ptr, uint8_t RXmessage[])
{
	ptr->angle = (uint16_t)(RXmessage[0]<<8 | RXmessage[1]) ;
	ptr->offset_angle = ptr->angle;
	
}


/*解析CAN回传的结构体，得到电机参数*/
void Get_moto_measure(moto_measure_t *ptr, uint8_t RXmessage[])
{
//	u32  sum=0;
//	u8	 i = FILTER_BUF_LEN;
	
	/*BUG!!! dont use this para code*/
//	ptr->angle_buf[ptr->buf_idx] = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
//	ptr->buf_idx = ptr->buf_idx++ > FILTER_BUF_LEN ? 0 : ptr->buf_idx;
//	while(i){
//		sum += ptr->angle_buf[--i];
//	}
//	ptr->fited_angle = sum / FILTER_BUF_LEN;
	ptr->last_angle = ptr->angle;
	ptr->angle = (uint16_t)(RXmessage[0]<<8 | RXmessage[1]) ;
	ptr->real_current  = (int16_t)(RXmessage[2]<<8 | RXmessage[3]);
	ptr->speed_rpm = ptr->real_current;	//这里是因为两种电调对应位不一样的信息
	ptr->given_current = (int16_t)(RXmessage[4]<<8 | RXmessage[5])/-5;
	ptr->hall =RXmessage[6];
	if(ptr->angle - ptr->last_angle > 4096)
		ptr->round_cnt --;
	else if (ptr->angle - ptr->last_angle < -4096)
		ptr->round_cnt ++;
	ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
	ptr->real_angle = ptr->total_angle / 22.75555556f;
}


#define ABS(x)	( (x>0) ? (x) : (-x) )
/**
*@bref 电机上电角度=0， 之后用这个函数更新3510电机的相对开机后（为0）的相对角度。
	*/
void Get_total_angle(moto_measure_t *p){
	
	int res1, res2, delta;
	if(p->angle < p->last_angle){			//可能的情况
		res1 = p->angle + 8192 - p->last_angle;	//正转，delta=+
		res2 = p->angle - p->last_angle;				//反转	delta=-
	}else{	//angle > last
		res1 = p->angle - 8192 - p->last_angle ;//反转	delta -
		res2 = p->angle - p->last_angle;				//正转	delta +
	}
	//不管正反转，肯定是转的角度小的那个是真的
	if(ABS(res1)<ABS(res2))
		delta = res1;
	else
		delta = res2;

	p->total_angle += delta;
	p->last_angle = p->angle;
}


    
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
   Count4++;
	if(hcan->Instance == CAN1)
    {
	
	while(HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0)
        {
			
            if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RXHeader, RXmessage) == HAL_OK)
            {
                uint8_t i;
                
                // 安全计算索引
                if(RXHeader.StdId >= CAN_3510Moto1_ID && RXHeader.StdId <= CAN_3510Moto2_ID) {
                    i = RXHeader.StdId - CAN_3510Moto_ALL_ID - 1;
                    
                    // 确保索引在有效范围内
                    if(i < 4) { // 定义MAX_MOTORS为电机数量
                        if(Moto_Measurement_value[i].msg_cnt <= 50) {
                            Get_moto_offset(&Moto_Measurement_value[i], RXmessage);
                        } else {
                            Get_moto_measure(&Moto_Measurement_value[i], RXmessage);
                        }
                        Moto_Measurement_value[i].msg_cnt++;
                    }
                }
            }
            else {
                // 处理接收错误
                break;
            }
        }
	}
	
	
//	if(hcan->Instance==CAN1)
//	{
//		HAL_CAN_GetRxMessage(hcan,CAN_FILTER_FIFO0,&RXHeader,RXmessage);//获取数据
//		switch(RXHeader.StdId)
//		{
//		          

//		case CAN_3510Moto1_ID:
//			{
//				static uint8_t i;
//				i = RXHeader.StdId - CAN_3510Moto_ALL_ID-1;
//				Moto_Measurement_value[i].msg_cnt++ <= 50	?	Get_moto_offset(&Moto_Measurement_value[i], RXmessage) : Get_moto_measure(&Moto_Measurement_value[i], RXmessage);
//			break;
//			}
//		case CAN_3510Moto2_ID:
//		{
//			
//				static uint8_t i;
//				i = RXHeader.StdId - CAN_3510Moto_ALL_ID-1;
//				
//				Moto_Measurement_value[i].msg_cnt++ <= 50	?	Get_moto_offset(&Moto_Measurement_value[i], RXmessage) : Get_moto_measure(&Moto_Measurement_value[i], RXmessage);
//			break;
//		}
//	}
// 
//    }
}

//void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan)
//{
//    uint32_t error_code = HAL_CAN_GetError(hcan);
//    Count3++;
//   
//    
//    if(error_code & CAN_IT_BUSOFF) {

//        // 总线离线，需要恢复
//        HAL_CAN_ResetError(hcan);
//        HAL_CAN_Stop(hcan);
//        HAL_Delay(100);
//        HAL_CAN_Start(hcan);
//        HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
//    }
//    
//    if(error_code & HAL_CAN_ERROR_EWG) {
////        printf("Error Warning State\n");
//    }
//    
//    if(error_code & HAL_CAN_ERROR_EPV) {
////        printf("Error Passive State\n");
//    }
//    
//    HAL_CAN_ResetError(hcan);
//}

void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan)
{
    uint32_t error_code = HAL_CAN_GetError(hcan);
    Count3++;
    
    if(error_code & CAN_IT_BUSOFF) {
        HAL_CAN_ResetError(hcan);  // 保持原位
        HAL_CAN_Stop(hcan);
        HAL_Delay(100);  // 延时让总线稳定
        HAL_CAN_Start(hcan);
        // 修复：重新激活完整通知（包括错误）
        HAL_CAN_ActivateNotification(hcan,
            CAN_IT_RX_FIFO0_MSG_PENDING |
            CAN_IT_ERROR |
            CAN_IT_BUSOFF |
            CAN_IT_LAST_ERROR_CODE);
        // 可选：重新配置过滤器（如果总线重置影响）
        // HAL_CAN_ConfigFilter(hcan, &sFilterConfig);  // 从init中移出，复用
    }
    
    if(error_code & HAL_CAN_ERROR_EWG) {
        // printf("Error Warning State\n");  // TEC/REC > 96
    }
    
    if(error_code & HAL_CAN_ERROR_EPV) {
        // printf("Error Passive State\n");  // TEC/REC > 127
    }
    
    HAL_CAN_ResetError(hcan);  // 始终清错误码
}
